//////////////////////////////////////////ok
#include"stdafx.h"

#include "bochs.h"


void IA32_CPU::stack_return_to_v86(Bit32u new_eip, Bit32u raw_cs_selector,Bit32u flags32)
{
  Bit32u temp_ESP, new_esp, esp_laddr;
  Bit16u raw_es_selector, raw_ds_selector, raw_fs_selector, raw_gs_selector, raw_ss_selector;

  if ( sregs[IA32_SEG_REG_SS].cache.u.segment.d_b)
    temp_ESP = ESP;
  else
    temp_ESP = SP;

  if ( !can_pop(36) ) 
  {
    exception(IA32_SS_EXCEPTION, 0, 0);
    return;
  }

  if ( new_eip & 0xffff0000 ) 
  {
    new_eip = new_eip & 0xffff;
  }

  esp_laddr =  sregs[IA32_SEG_REG_SS].cache.u.segment.base +temp_ESP;
  m_vm32->access_address_n(this,esp_laddr + 12, 4, 0, IA32_READ, &new_esp);
  m_vm32->access_address_n(this,esp_laddr + 16, 2, 0, IA32_READ, &raw_ss_selector);
  m_vm32->access_address_n(this,esp_laddr + 20, 2, 0, IA32_READ, &raw_es_selector);
  m_vm32->access_address_n(this,esp_laddr + 24, 2, 0, IA32_READ, &raw_ds_selector);
  m_vm32->access_address_n(this,esp_laddr + 28, 2, 0, IA32_READ, &raw_fs_selector);
  m_vm32->access_address_n(this,esp_laddr + 32, 2, 0, IA32_READ, &raw_gs_selector);
#ifdef IA32_BIG_ENDIAN
  IA32_ReadHostDWordFromLittleEndian(&new_esp, &new_esp);
  IA32_ReadHostWordFromLittleEndian(&raw_ss_selector, &raw_ss_selector);
  IA32_ReadHostWordFromLittleEndian(&raw_es_selector, &raw_es_selector);
  IA32_ReadHostWordFromLittleEndian(&raw_ds_selector, &raw_ds_selector);
  IA32_ReadHostWordFromLittleEndian(&raw_fs_selector, &raw_fs_selector);
  IA32_ReadHostWordFromLittleEndian(&raw_gs_selector, &raw_gs_selector);
#endif
  write_eflags(flags32, /*change IOPL*/ 1, /*change IF*/ 1, /*change VM*/ 1, /*change RF*/ 1);

   sregs[IA32_SEG_REG_CS].selector.value = raw_cs_selector;
  EIP = new_eip;
   sregs[IA32_SEG_REG_ES].selector.value = raw_es_selector;
   sregs[IA32_SEG_REG_DS].selector.value = raw_ds_selector;
   sregs[IA32_SEG_REG_FS].selector.value = raw_fs_selector;
   sregs[IA32_SEG_REG_GS].selector.value = raw_gs_selector;
   sregs[IA32_SEG_REG_SS].selector.value = raw_ss_selector;
  ESP = new_esp; // Full 32bits are loaded.
  init_v8086_mode();
}

void IA32_CPU::stack_return_from_v86(Ia32_Instruction_c *i)
{
  if ( get_IOPL() != 3) 
  {
    exception(IA32_GP_EXCEPTION, 0, 0);
    return;
  }

  if (i->os32L()) 
  {
    Bit32u eip, ecs_raw, eflags_tmp;
    if( !can_pop(12) )
    {
      exception(IA32_SS_EXCEPTION, 0, 0);
      return;
    }
  
    pop_32(&eip);
    pop_32(&ecs_raw);
    pop_32(&eflags_tmp);
    load_seg_reg(& sregs[IA32_SEG_REG_CS], (Bit16u) ecs_raw);
    EIP = eip;
    write_eflags(eflags_tmp, /*IOPL*/ 0, /*IF*/ 1, /*VM*/ 0, /*RF*/ 1);
  }
  else 
  {
    Bit16u ip, cs_raw, flags;
    if( !can_pop(6) )
    {
      exception(IA32_SS_EXCEPTION, 0, 0);
      return;
    }

    pop_16(&ip);
    pop_16(&cs_raw);
    pop_16(&flags);
    load_seg_reg(& sregs[IA32_SEG_REG_CS], cs_raw);
    EIP = (Bit32u) ip;
    write_flags(flags, /*IOPL*/ 0, /*IF*/ 1);
  }
}

void IA32_CPU::init_v8086_mode(void)
{
   sregs[IA32_SEG_REG_CS].cache.valid                  = 1;
   sregs[IA32_SEG_REG_CS].cache.p                      = 1;
   sregs[IA32_SEG_REG_CS].cache.dpl                    = 3;
   sregs[IA32_SEG_REG_CS].cache.segment                = 1;
   sregs[IA32_SEG_REG_CS].cache.u.segment.executable   = 1;
   sregs[IA32_SEG_REG_CS].cache.u.segment.c_ed         = 0;
   sregs[IA32_SEG_REG_CS].cache.u.segment.r_w          = 1;
   sregs[IA32_SEG_REG_CS].cache.u.segment.a            = 1;
   sregs[IA32_SEG_REG_CS].cache.u.segment.base         =
     sregs[IA32_SEG_REG_CS].selector.value << 4;
   sregs[IA32_SEG_REG_CS].cache.u.segment.limit        = 0xffff;
   sregs[IA32_SEG_REG_CS].cache.u.segment.limit_scaled = 0xffff;
   sregs[IA32_SEG_REG_CS].cache.u.segment.g            = 0;
   sregs[IA32_SEG_REG_CS].cache.u.segment.d_b          = 0;
   sregs[IA32_SEG_REG_CS].cache.u.segment.avl          = 0;
   sregs[IA32_SEG_REG_CS].selector.rpl                 = 3;

   sregs[IA32_SEG_REG_SS].cache.valid                  = 1;
   sregs[IA32_SEG_REG_SS].cache.p                      = 1;
   sregs[IA32_SEG_REG_SS].cache.dpl                    = 3;
   sregs[IA32_SEG_REG_SS].cache.segment                = 1;
   sregs[IA32_SEG_REG_SS].cache.u.segment.executable   = 0;
   sregs[IA32_SEG_REG_SS].cache.u.segment.c_ed         = 0;
   sregs[IA32_SEG_REG_SS].cache.u.segment.r_w          = 1;
   sregs[IA32_SEG_REG_SS].cache.u.segment.a            = 1;
   sregs[IA32_SEG_REG_SS].cache.u.segment.base         =
     sregs[IA32_SEG_REG_SS].selector.value << 4;
   sregs[IA32_SEG_REG_SS].cache.u.segment.limit        = 0xffff;
   sregs[IA32_SEG_REG_SS].cache.u.segment.limit_scaled = 0xffff;
   sregs[IA32_SEG_REG_SS].cache.u.segment.g            = 0;
   sregs[IA32_SEG_REG_SS].cache.u.segment.d_b          = 0;
   sregs[IA32_SEG_REG_SS].cache.u.segment.avl          = 0;
   sregs[IA32_SEG_REG_SS].selector.rpl                 = 3;

   sregs[IA32_SEG_REG_ES].cache.valid                  = 1;
   sregs[IA32_SEG_REG_ES].cache.p                      = 1;
   sregs[IA32_SEG_REG_ES].cache.dpl                    = 3;
   sregs[IA32_SEG_REG_ES].cache.segment                = 1;
   sregs[IA32_SEG_REG_ES].cache.u.segment.executable   = 0;
   sregs[IA32_SEG_REG_ES].cache.u.segment.c_ed         = 0;
   sregs[IA32_SEG_REG_ES].cache.u.segment.r_w          = 1;
   sregs[IA32_SEG_REG_ES].cache.u.segment.a            = 1;
   sregs[IA32_SEG_REG_ES].cache.u.segment.base         =
     sregs[IA32_SEG_REG_ES].selector.value << 4;
   sregs[IA32_SEG_REG_ES].cache.u.segment.limit        = 0xffff;
   sregs[IA32_SEG_REG_ES].cache.u.segment.limit_scaled = 0xffff;
   sregs[IA32_SEG_REG_ES].cache.u.segment.g            = 0;
   sregs[IA32_SEG_REG_ES].cache.u.segment.d_b          = 0;
   sregs[IA32_SEG_REG_ES].cache.u.segment.avl          = 0;
   sregs[IA32_SEG_REG_ES].selector.rpl                 = 3;

   sregs[IA32_SEG_REG_DS].cache.valid                  = 1;
   sregs[IA32_SEG_REG_DS].cache.p                      = 1;
   sregs[IA32_SEG_REG_DS].cache.dpl                    = 3;
   sregs[IA32_SEG_REG_DS].cache.segment                = 1;
   sregs[IA32_SEG_REG_DS].cache.u.segment.executable   = 0;
   sregs[IA32_SEG_REG_DS].cache.u.segment.c_ed         = 0;
   sregs[IA32_SEG_REG_DS].cache.u.segment.r_w          = 1;
   sregs[IA32_SEG_REG_DS].cache.u.segment.a            = 1;
   sregs[IA32_SEG_REG_DS].cache.u.segment.base         =
     sregs[IA32_SEG_REG_DS].selector.value << 4;
   sregs[IA32_SEG_REG_DS].cache.u.segment.limit        = 0xffff;
   sregs[IA32_SEG_REG_DS].cache.u.segment.limit_scaled = 0xffff;
   sregs[IA32_SEG_REG_DS].cache.u.segment.g            = 0;
   sregs[IA32_SEG_REG_DS].cache.u.segment.d_b          = 0;
   sregs[IA32_SEG_REG_DS].cache.u.segment.avl          = 0;
   sregs[IA32_SEG_REG_DS].selector.rpl                 = 3;

   sregs[IA32_SEG_REG_FS].cache.valid                  = 1;
   sregs[IA32_SEG_REG_FS].cache.p                      = 1;
   sregs[IA32_SEG_REG_FS].cache.dpl                    = 3;
   sregs[IA32_SEG_REG_FS].cache.segment                = 1;
   sregs[IA32_SEG_REG_FS].cache.u.segment.executable   = 0;
   sregs[IA32_SEG_REG_FS].cache.u.segment.c_ed         = 0;
   sregs[IA32_SEG_REG_FS].cache.u.segment.r_w          = 1;
   sregs[IA32_SEG_REG_FS].cache.u.segment.a            = 1;
   sregs[IA32_SEG_REG_FS].cache.u.segment.base         =
     sregs[IA32_SEG_REG_FS].selector.value << 4;
   sregs[IA32_SEG_REG_FS].cache.u.segment.limit        = 0xffff;
   sregs[IA32_SEG_REG_FS].cache.u.segment.limit_scaled = 0xffff;
   sregs[IA32_SEG_REG_FS].cache.u.segment.g            = 0;
   sregs[IA32_SEG_REG_FS].cache.u.segment.d_b          = 0;
   sregs[IA32_SEG_REG_FS].cache.u.segment.avl          = 0;
   sregs[IA32_SEG_REG_FS].selector.rpl                 = 3;

   sregs[IA32_SEG_REG_GS].cache.valid                  = 1;
   sregs[IA32_SEG_REG_GS].cache.p                      = 1;
   sregs[IA32_SEG_REG_GS].cache.dpl                    = 3;
   sregs[IA32_SEG_REG_GS].cache.segment                = 1;
   sregs[IA32_SEG_REG_GS].cache.u.segment.executable   = 0;
   sregs[IA32_SEG_REG_GS].cache.u.segment.c_ed         = 0;
   sregs[IA32_SEG_REG_GS].cache.u.segment.r_w          = 1;
   sregs[IA32_SEG_REG_GS].cache.u.segment.a            = 1;
   sregs[IA32_SEG_REG_GS].cache.u.segment.base         =
     sregs[IA32_SEG_REG_GS].selector.value << 4;
   sregs[IA32_SEG_REG_GS].cache.u.segment.limit        = 0xffff;
   sregs[IA32_SEG_REG_GS].cache.u.segment.limit_scaled = 0xffff;
   sregs[IA32_SEG_REG_GS].cache.u.segment.g            = 0;
   sregs[IA32_SEG_REG_GS].cache.u.segment.d_b          = 0;
   sregs[IA32_SEG_REG_GS].cache.u.segment.avl          = 0;
   sregs[IA32_SEG_REG_GS].selector.rpl                 = 3;
}

